package localSearch;

import fundamental.*;

import javax.swing.plaf.IconUIResource;
import java.util.ArrayList;
import java.util.HashSet;
import java.util.Iterator;

import static fundamental.Params.*;

public class LocalSearch {

    public static void restoreCandLoading() {
        for (Cand cand: openedCandSet
        ) {
            cand.loading += cand.overLoading;
            cand.overLoading = 0;
        }
    }
    /**
     *  relocation算子做第二级路径，cand的overload全部转移到loading上
     */
    public static void relocate2() {
        Customer bestCust = null;
        SecondVehicle bestOutV = null;
        int bestOutInd = -1;
        SecondVehicle bestInV = null;
        int bestInInd = -1;
        double bestDeltaCost = 0;
        for (SecondVehicle v1: secondVehicleList
        ) {
            if (v1.route.size() > 2) {
                for (int i = 1; i < v1.route.size()-1; i++) {
                    Customer cust = (Customer) v1.route.get(i);
                    Point last = v1.route.get(i-1);
                    Point next = v1.route.get(i+1);
                    v1.route.remove(i);
                    v1.start_end.loading -= (cust.demand+cust.overDemand);
                    v1.loading -= cust.demand;
                    double minusVehicleCost = 0;
                    if (cust.sameCand != v1.start_end) {
                        minusVehicleCost = getMinusVFixCost(v1);
                    }
                    double minusCandCost = getMinusCFixCost(v1);
                    minusVehicleCost += minusCandCost;
                    double minusTravelCost = getMinusTravelCost(v1,cust,last,next);
                    for (SecondVehicle v2: secondVehicleList
                    ) {
                        if (v2.route.size() == 3) {
                            Cand cand = (Cand) v2.route.get(0);
                            Customer cust1 = (Customer) v2.route.get(1);
                            if (cand.sameCust == cust1) continue;
                        }
                        if (v2.loading + cust.demand <= v2.cap) {
                            for (int j = 1; j < v2.route.size(); j++) {
                                Point before = v2.route.get(j-1);
                                Point after = v2.route.get(j);
                                if (dist[before.num][cust.num] <= distThres2 && dist[cust.num][after.num] <= distThres2) {
                                    double addVehicleCost = 0;
                                    if (cust.sameCand != v2.start_end) {
                                        addVehicleCost = getAddVFixCost(v2);
                                    }else if (v2.route.size() != 2) continue;
                                    if (v1.start_end == v2.start_end) addVehicleCost += minusCandCost;
                                    else addVehicleCost += getAddCFixCost(v2);
                                    double addTravelCost = getAddTravelCost(v2, cust,before,after);
                                    double deltaVehicleCost = addVehicleCost - minusVehicleCost;
                                    double deltaTravelCost = addTravelCost - minusTravelCost;
                                    double deltaCost = deltaVehicleCost + deltaTravelCost;
                                    if (bestDeltaCost - deltaCost > tolerance) {
                                        bestDeltaCost = deltaCost;
                                        bestCust = cust;
                                        bestOutV = v1;
                                        bestOutInd = i;
                                        bestInV = v2;
                                        bestInInd = j;
                                    }
                                }

                            }
                        }
                    }
                    v1.route.add(i, cust);
                    v1.loading += cust.demand;
                    v1.start_end.loading += (cust.demand+cust.overDemand);
                }
            }
        }
        if (bestDeltaCost < -tolerance) {
            operIsUpdate = true;
            secCost += bestDeltaCost;
            bestOutV.route.remove(bestOutInd);
            bestInV.route.add(bestInInd, bestCust);
            bestOutV.loading -= bestCust.demand;
            bestInV.loading += bestCust.demand;
            bestOutV.start_end.loading -= (bestCust.demand+bestCust.overDemand);
            bestInV.start_end.loading += (bestCust.demand+bestCust.overDemand);
            if (bestOutV.start_end.loading == 0) openedCandSet.remove(bestOutV.start_end);
        }
    }

    /**
     * 2opt算子用于第二级路径：该算子不计算虚拟路径
     */
    public static void twoOpt2() {
        double bestDeltaCost = Integer.MAX_VALUE;
        SecondVehicle bestV1 = null;
        SecondVehicle bestV2 = null;
        int indexV1 = -1;
        int indexV2 = -1;
        int bestLoadV1 = 0;
        int bestLoadV2 = 0;
        int bestTotalLoadV1 = 0;
        int bestTotalLoadV2 = 0;
        int bestTotalRemove1 = 0;
        int bestTotalRemove2 = 0;
        int bestSignal = -1;
        boolean bestInverse = false;
        Cand bestV1Cand = null;  //  第一辆车需要更换的cand（指v1尾+v2头 或 v1头+v2头）
        Cand bestV2Cand = null;  //  第二辆车需要更换的cand（指v1头+v2尾 或 v1尾+v2尾）
        ArrayList<ArrayList<Integer>> accLoad = new ArrayList<>();
        ArrayList<ArrayList<Integer>> accTotalLoad = new ArrayList<>();
        getLoadSegment(accLoad, accTotalLoad);
        for (int u = 0; u < secondVehicleList.size(); u++) {
            SecondVehicle v1 = secondVehicleList.get(u);
            if (v1.route.size() == 3 && v1.route.get(1) == v1.start_end.sameCust) continue;
            for (int i = 1; i < v1.route.size(); i++) {
                /**
                 * 可以采用segment的方法进行时间复杂度优化
                 */
                int loadV11 = accLoad.get(u).get(i-1);
                int loadV12 = accLoad.get(u).get(v1.route.size()-1) - loadV11;
                int totalLoadV11 = accTotalLoad.get(u).get(i-1);
                int totalLoadV12 = accTotalLoad.get(u).get(v1.route.size()-1) - totalLoadV11;
                /*for (int i1 = 1; i1 < i; i1++) {
                    loadV11 += ((Customer)v1.route.get(i1)).demand;
                    totalLoadV11 += ((Customer)v1.route.get(i1)).demand+((Customer)v1.route.get(i1)).overDemand;
                }
                for (int i1 = i; i1 < v1.route.size()-1; i1++) {
                    loadV12 += ((Customer)v1.route.get(i1)).demand;
                    totalLoadV12 += ((Customer)v1.route.get(i1)).demand+((Customer)v1.route.get(i1)).overDemand;
                }*/
                for (int v = u; v < secondVehicleList.size(); v++) {
                    SecondVehicle v2 = secondVehicleList.get(v);
                    if (v2.route.size() == 3 && v2.route.get(1) == v2.start_end.sameCust) continue;
                    for (int j = 1; j < v2.route.size(); j++) {
                        /**
                         * 可以采用segment的方法进行时间复杂度优化
                         */
                        int loadV21 = accLoad.get(v).get(j-1);
                        int loadV22 = accLoad.get(v).get(v2.route.size()-1) - loadV21;
                        int totalLoadV21 = accTotalLoad.get(v).get(j-1);
                        int totalLoadV22 = accTotalLoad.get(v).get(v2.route.size()-1) - totalLoadV21;
                        /*for (int j1 = 1; j1 < j; j1++) {
                            loadV21 += ((Customer)v2.route.get(j1)).demand;
                            totalLoadV21 += ((Customer)v2.route.get(j1)).demand+((Customer)v2.route.get(j1)).overDemand;
                        }
                        for (int j1 = j; j1 < v2.route.size()-1; j1++) {
                            loadV22 += ((Customer)v2.route.get(j1)).demand;
                            totalLoadV22 += ((Customer)v2.route.get(j1)).demand+((Customer)v2.route.get(j1)).overDemand;
                        }*/
                        double deltaCost = Integer.MAX_VALUE;
                        boolean inverse = false;
                        int signal = -1;
                        int loadV1 = 0;
                        int loadV2 = 0;
                        int totalLoadV1 = 0;
                        int totalLoadV2 = 0;
                        Cand cand1 = null;
                        Cand cand2 = null;
                        if (u == v && i < j) {
                            signal = 0;
                            deltaCost = (dist[v1.route.get(i - 1).num][v2.route.get(j - 1).num] + dist[v1.route.get(i).num][v2.route.get(j).num] - dist[v1.route.get(i - 1).num][v1.route.get(i).num] - dist[v2.route.get(j - 1).num][v2.route.get(j).num]) * v2.vcost;
                        }else if (u < v){
                            Cand cand11 = null;  //  第一种类型第一个路径的cand
                            Cand cand12 = null;  //  第一种类型第二个路径的cand
                            Cand cand21 = null;  //  第二种类型第一个路径的cand
                            Cand cand22 = null;  //  第二种类型第二个路径的cand
                            signal = 2;
                            double deltaCost1 = Double.MAX_VALUE;
                            double deltaCost2 = Double.MAX_VALUE;
                            //  头尾拼接型
                            if (loadV11 + loadV22 <= v1.cap && loadV12 + loadV21 <= v2.cap && dist[v1.route.get(i-1).num][v2.route.get(j).num] <= distThres2 && dist[v1.route.get(i).num][v2.route.get(j-1).num] <= distThres2) {
                                deltaCost1 = (dist[v1.route.get(i-1).num][v2.route.get(j).num]+dist[v1.route.get(i).num][v2.route.get(j-1).num]-dist[v1.route.get(i-1).num][v1.route.get(i).num]-dist[v2.route.get(j-1).num][v2.route.get(j).num])*v1.vcost;

                                double add1;
                                double add2;
                                //  第一条尾+第二条头的路径，将start_end换成v2的，即更换第一条路径的尾
                                if (i == v1.route.size()-1){
                                    add1 = (dist[v2.route.get(j-1).num][v2.start_end.num]-dist[v2.route.get(j-1).num][v1.start_end.num])*v1.vcost;
                                }else {
                                    add1 = (dist[v1.route.get(v1.route.size()-2).num][v2.start_end.num]-dist[v1.route.get(v1.route.size()-2).num][v1.start_end.num])*v2.vcost;
                                }
                                add1 = getOverTravelCost(add1,v1,v2.start_end,i,v1.route.size()-1);
                                //  第一条尾+第二条头的路径，将start_end换成v1的，即更换第二条路径的头
                                if (j == 1){
                                    //  这种情况，拼接后是v1的i节点与v2.start_end连接
                                    add2 = (dist[v1.route.get(i).num][v1.start_end.num]-dist[v1.route.get(i).num][v2.start_end.num])*v2.vcost;
                                }else {
                                    add2 = (dist[v2.route.get(1).num][v1.start_end.num]-dist[v2.route.get(1).num][v2.start_end.num])*v2.vcost;
                                }
                                add2 = getOverTravelCost(add2,v2,v1.start_end,1,j);
                                //  确定第一条尾+第二条头拼接的路径用什么cand最好
                                if (add1 < add2) {
                                    deltaCost1 += add1;
                                    cand11 = v2.start_end;
                                }else {
                                    deltaCost1 += add2;
                                    cand11 = v1.start_end;
                                }

                                //  第一条头+第二条尾的路径，将start_end换成v1的，即更换第二条路径的尾
                                if (j == v2.route.size()-1){
                                    //  这种情况，拼接后是v1的i-1节点与v2.start_end连接
                                    add1 = (dist[v1.route.get(i-1).num][v1.start_end.num]-dist[v1.route.get(i-1).num][v2.start_end.num])*v1.vcost;
                                }else {
                                    add1 = (dist[v2.route.get(v2.route.size()-2).num][v1.start_end.num]-dist[v2.route.get(v2.route.size()-2).num][v2.start_end.num])*v2.vcost;
                                }
                                add1 = getOverTravelCost(add1, v2, v1.start_end, j, v2.route.size()-1);
                                //  第一条头+第二条尾的路径，将start_end换成v2的，即更换第一条路径的头
                                if (i == 1) {
                                    //  这种情况，拼接后是v2的j节点与v1.start_end连接
                                    add2 = (dist[v2.route.get(j).num][v2.start_end.num]-dist[v2.route.get(j).num][v1.start_end.num])*v1.vcost;
                                }else {
                                    add2 = (dist[v1.route.get(1).num][v2.start_end.num]-dist[v1.route.get(1).num][v1.start_end.num])*v1.vcost;
                                }
                                add2 = getOverTravelCost(add2,v1,v2.start_end,1,i);
                                //  确定第一条头+第二条尾拼接的路径用什么cand最好
                                if (add1 < add2) {
                                    deltaCost1 += add1;
                                    cand12 = v1.start_end;
                                }else {
                                    deltaCost1 += add2;
                                    cand12 = v2.start_end;
                                }
                                //  车辆固定成本的变动（由于该算子不会对虚拟路径进行处理，因此不会有选址成本的变动）
                                if (i == 1 && j == v2.route.size()-1) deltaCost1 -= vFix2;
                                if (i == v1.route.size()-1 && j == 1) deltaCost1 -= vFix2;
                                if (v1.route.size() == 2) deltaCost1 += vFix2;
                                if (v2.route.size() == 2) deltaCost1 += vFix2;

                            }
                            //  头头尾尾拼接型
                            if (loadV11 + loadV21 <= v1.cap && loadV12 + loadV22 <= v2.cap && dist[v1.route.get(i-1).num][v2.route.get(j-1).num] <= distThres2 && dist[v1.route.get(i).num][v2.route.get(j).num] <= distThres2) {
                                deltaCost2 = (dist[v1.route.get(i-1).num][v2.route.get(j-1).num]+dist[v1.route.get(i).num][v2.route.get(j).num]-dist[v1.route.get(i-1).num][v1.route.get(i).num]-dist[v2.route.get(j-1).num][v2.route.get(j).num])*v1.vcost;
                                /*if (v1.route.get(i-1).num == 236 && v2.route.get(j).num == 1072) {
                                    System.out.println(" ");
                                    System.out.println("断路重连的成本增量为："+deltaCost2);
                                }*/
                                double add1;
                                double add2;
                                //  第一条头+第二条头的路径，将start_end换成v2的，即更换第一条路径的头
                                if (i == 1) {
                                    add1 = (dist[v2.route.get(j-1).num][v2.start_end.num]-dist[v2.route.get(j-1).num][v1.start_end.num])*v1.vcost;
                                }else {
                                    add1 = (dist[v1.route.get(1).num][v2.start_end.num]-dist[v1.route.get(1).num][v1.start_end.num])*v1.vcost;
                                }
                                add1 = getOverTravelCost(add1, v1, v2.start_end, 1, i);
                                //  第一条头+第二条头的路径，将start_end换成v1的，即更换第二条路径的头
                                if (j == 1) {
                                    add2 = (dist[v1.route.get(i-1).num][v1.start_end.num]-dist[v1.route.get(i-1).num][v2.start_end.num])*v1.vcost;
                                    //if (v1.route.get(i-1).num == 236 && v2.route.get(j).num == 1072) System.out.println("第一条路径变化cand后的成本增量："+add2);
                                }else {
                                    add2 = (dist[v2.route.get(1).num][v1.start_end.num]-dist[v2.route.get(1).num][v2.start_end.num])*v1.vcost;
                                }
                                add2 = getOverTravelCost(add2, v2, v1.start_end, 1, j);
                                //if (v1.route.get(i-1).num == 236 && v2.route.get(j).num == 1072) System.out.println("第一条路径变化cand后和考虑超量的成本增量："+add2);
                                //  确定第一条头+第二条头拼接的路径用什么cand最好
                                if (add1 < add2) {
                                    deltaCost2 += add1;
                                    cand21 = v2.start_end;
                                }else {
                                    deltaCost2 += add2;
                                    cand21 = v1.start_end;
                                }

                                //  第一条尾+第二条尾的路径，将start_end换成v2的，即更换第一条路径的尾
                                if (i == v1.route.size()-1) {
                                    add1 = (dist[v2.route.get(j).num][v2.start_end.num]-dist[v2.route.get(j).num][v1.start_end.num])*v1.vcost;
                                }else {
                                    add1 = (dist[v1.route.get(v1.route.size()-2).num][v2.start_end.num]-dist[v1.route.get(v1.route.size()-2).num][v1.start_end.num])*v1.vcost;
                                }
                                add1 = getOverTravelCost(add1, v1, v2.start_end, i, v1.route.size()-1);
                                //  第一条尾+第二条尾的路径，将start_end换成v1的，即更换第二条路径的尾
                                if (j == v2.route.size()-1) {
                                    add2 = (dist[v1.route.get(i).num][v1.start_end.num]-dist[v1.route.get(i).num][v2.start_end.num])*v1.vcost;
                                }else {
                                    add2 = (dist[v2.route.get(v2.route.size()-2).num][v1.start_end.num]-dist[v2.route.get(v2.route.size()-2).num][v2.start_end.num])*v1.vcost;
                                    //if (v1.route.get(i-1).num == 236 && v2.route.get(j).num == 1072) System.out.println("第二条路径变化cand后的成本增量："+add2);
                                }
                                add2 = getOverTravelCost(add2, v2, v1.start_end, j, v2.route.size()-1);
                                //if (v1.route.get(i-1).num == 236 && v2.route.get(j).num == 1072) System.out.println("第二条路径变化cand后和考虑超量的成本增量："+add2);
                                //  确定第一条尾+第二条尾拼接的路径用什么cand最好
                                if (add1 < add2) {
                                    deltaCost2 += add1;
                                    cand22 = v2.start_end;
                                }else {
                                    deltaCost2 += add2;
                                    cand22 = v1.start_end;
                                    /*if (v1.route.get(i).num == 1916 && v2.route.get(j).num == 1616) {
                                        System.out.println("第二条路径变化cand后的成本增量："+add2);
                                        System.out.println("v2的j点："+v2.route.get(j).num);
                                        System.out.println("v1的start点："+v1.start_end.num);
                                        System.out.println("v2的start点："+v2.start_end.num);
                                        System.out.println("v2的j到v1的start："+dist[v2.route.get(j).num][v1.start_end.num]);
                                        System.out.println("v2的j到v2的start："+dist[v2.route.get(j).num][v2.start_end.num]);
                                        System.out.println();
                                    }*/
                                }
                                //  车辆固定成本的变动（由于该算子不会对虚拟路径进行处理，因此不会有选址成本的变动）
                                if (i == 1 && j == 1) deltaCost2 -= vFix2;
                                if (i == v1.route.size()-1 && j == v2.route.size()-1) deltaCost2 -= vFix2;
                                if (v1.route.size() == 2) deltaCost2 += vFix2;
                                if (v2.route.size() == 2) deltaCost2 += vFix2;
                            }
                            if (deltaCost1 < deltaCost2) {
                                deltaCost = deltaCost1;
                                loadV1 = loadV12 + loadV21;
                                loadV2 = loadV11 + loadV22;
                                totalLoadV1 = totalLoadV12 + totalLoadV21;
                                totalLoadV2 = totalLoadV11 + totalLoadV22;
                                cand1 = cand11;
                                cand2 = cand12;

                            }else {
                                deltaCost = deltaCost2;
                                loadV1 = loadV11 + loadV21;
                                loadV2 = loadV12 + loadV22;
                                totalLoadV1 = totalLoadV11 + totalLoadV21;
                                totalLoadV2 = totalLoadV12 + totalLoadV22;
                                inverse = true;
                                cand1 = cand21;
                                cand2 = cand22;
                            }
                        }
                        if (bestDeltaCost - deltaCost > tolerance) {
                            bestDeltaCost = deltaCost;
                            bestV1 = v1;
                            bestV2 = v2;
                            indexV1 = i;
                            indexV2 = j;
                            bestLoadV1 = loadV1;
                            bestLoadV2 = loadV2;
                            bestTotalLoadV1 = totalLoadV1;
                            bestTotalLoadV2 = totalLoadV2;
                            bestTotalRemove1 = totalLoadV11+totalLoadV12;
                            bestTotalRemove2 = totalLoadV21+totalLoadV22;
                            bestSignal = signal;
                            bestInverse = inverse;
                            bestV1Cand = cand1;
                            bestV2Cand = cand2;
                        }
                    }
                }
            }
        }
        //System.out.println("成本核实没问题");
        if (bestDeltaCost < -tolerance) {
            operIsUpdate = true;
            secCost += bestDeltaCost;
            if (bestSignal == 0) {
                indexV2 -= 1;
                while (indexV1 < indexV2) {
                    Point temp = bestV1.route.get(indexV1);
                    bestV1.route.set(indexV1, bestV1.route.get(indexV2));
                    bestV1.route.set(indexV2, temp);
                    indexV1++;
                    indexV2--;
                }
            }else {
                /*System.out.println("bestV1="+bestV1.route);
                System.out.println("bestV2="+bestV2.route);
                System.out.println("bestCand1="+bestV1Cand);
                System.out.println("bestCand2="+bestV2Cand);
                System.out.println("loading1="+bestV1.loading);
                System.out.println("loading2="+bestV2.loading);*/
                Cand remove1 = bestV1.start_end;
                Cand remove2 = bestV2.start_end;
                bestV1.start_end.loading -= bestTotalRemove1;
                bestV2.start_end.loading -= bestTotalRemove2;
                ArrayList<Point> tempRoute1 = new ArrayList<>(bestV1.route);
                ArrayList<Point> tempRoute2 = new ArrayList<>(bestV2.route);
                bestV1.route.clear();
                bestV2.route.clear();
                if (!bestInverse) {
                    for (int i = 0; i < indexV2; i++) {
                        bestV1.route.add(tempRoute2.get(i));
                    }
                    for (int i = indexV1; i < tempRoute1.size(); i++) {
                        bestV1.route.add(tempRoute1.get(i));
                    }
                    for (int i = 0; i < indexV1; i++) {
                        bestV2.route.add(tempRoute1.get(i));
                    }
                    for (int i = indexV2; i < tempRoute2.size(); i++) {
                        bestV2.route.add(tempRoute2.get(i));
                    }
                }else {
                    for (int i = 0; i < indexV1; i++) {
                        bestV1.route.add(tempRoute1.get(i));
                    }
                    for (int i = indexV2-1; i >= 0; i--) {
                        bestV1.route.add(tempRoute2.get(i));
                    }
                    for (int i = tempRoute2.size()-1; i >= indexV2; i--) {
                        bestV2.route.add(tempRoute2.get(i));
                    }
                    for (int i = indexV1; i < tempRoute1.size(); i++) {
                        bestV2.route.add(tempRoute1.get(i));
                    }
                }
                bestV1.loading = bestLoadV1;
                bestV2.loading = bestLoadV2;
                bestV1.route.set(0, bestV1Cand);
                bestV1.route.set(bestV1.route.size()-1, bestV1Cand);
                bestV2.route.set(0, bestV2Cand);
                bestV2.route.set(bestV2.route.size()-1, bestV2Cand);
                bestV1.start_end = bestV1Cand;
                bestV2.start_end = bestV2Cand;
                bestV1Cand.loading += bestTotalLoadV1;
                bestV2Cand.loading += bestTotalLoadV2;
                if (remove1.loading == 0) openedCandSet.remove(remove1);
                if (remove2.loading == 0) openedCandSet.remove(remove2);
                /*try {
                    Check.check2();
                    Check.checkCost2();
                }catch (RuntimeException e) {
                    System.out.println("index1="+indexV1);
                    System.out.println("index2="+indexV2);
                    System.out.println("inverse="+bestInverse);
                    System.out.println("bestV1="+bestV1.route);
                    System.out.println("bestV2="+bestV2.route);
                    System.out.println(remove1.num+"loading="+remove1.loading);
                    System.out.println(remove2.num+"loading="+remove2.loading);
                    System.exit(-1);
                }*/
            }
        }
    }

    private static void getLoadSegment(ArrayList<ArrayList<Integer>> accLoad, ArrayList<ArrayList<Integer>> accTotalLoad) {
        for (SecondVehicle v: secondVehicleList
             ) {
            ArrayList<Integer> vLoad = new ArrayList<>();
            ArrayList<Integer> vTotalLoad = new ArrayList<>();
            int load = 0;
            int totalLoad = 0;
            for (Point p: v.route
                 ) {
                if (p instanceof Customer) {
                    Customer cust = (Customer) p;
                    load += cust.demand;
                    totalLoad += cust.demand + cust.overDemand;
                }
                vLoad.add(load);
                vTotalLoad.add(totalLoad);
            }
            accLoad.add(vLoad);
            accTotalLoad.add(vTotalLoad);
        }
    }

    /**
     * 当客户移动时，超额需求也会移动，因此需要计算超额需求移动的成本。这个可使用segment方法减少时间复杂度
     * @param cost 累积成本增量
     * @param v 待移动的车
     * @param cand 目标cand
     * @param l 开始索引
     * @param r 结束索引
     * @return
     */
    private static double getOverTravelCost(double cost, SecondVehicle v, Cand cand, int l, int r) {
        for (int i = l; i < r; i++) {
            Customer cust = (Customer) v.route.get(i);
            cost += (double)(cust.overDemand/v.cap)*2*v.vcost*(dist[cand.num][cust.num]-dist[v.start_end.num][cust.num]);
        }
        return cost;
    }

    /**
     * relocation算子做第一级路径
     */
    public static void relocate1() {
        Cand bestCand = null;
        FirstVehicle bestOutV = null;
        int bestOutInd = -1;
        FirstVehicle bestInV = null;
        int bestInInd = -1;
        double bestDeltaCost = 0;
        for (FirstVehicle v1: firstVehicleList
        ) {
            if (v1.route.size() > 2) {
                for (int i = 1; i < v1.route.size()-1; i++) {
                    Cand cand = (Cand) v1.route.get(i);
                    Point last = v1.route.get(i-1);
                    Point next = v1.route.get(i+1);
                    v1.route.remove(i);
                    v1.loading -= cand.loading;
                    double minusVehicleCost = getMinusVFixCost(v1);
                    double minusTravelCost = getMinusTravelCost(v1,cand,last,next);
                    for (FirstVehicle v2: firstVehicleList
                    ) {
                        if (v2.route.size() < 4 && v2.loading + cand.loading <= v2.cap) {
                            for (int j = 1; j < v2.route.size(); j++) {
                                Point before = v2.route.get(j-1);
                                Point after = v2.route.get(j);
                                if (dist[before.num][cand.num] <= distThres1 && dist[cand.num][after.num] <= distThres1) {
                                    double addVehicleCost = getAddVFixCost(v2);
                                    double addTravelCost = getAddTravelCost(v2, cand,before,after);
                                    double deltaVehicleCost = addVehicleCost - minusVehicleCost;
                                    double deltaTravelCost = addTravelCost - minusTravelCost;
                                    double deltaCost = deltaVehicleCost + deltaTravelCost;
                                    if (bestDeltaCost - deltaCost > tolerance) {
                                        bestDeltaCost = deltaCost;
                                        bestCand = cand;
                                        bestOutV = v1;
                                        bestOutInd = i;
                                        bestInV = v2;
                                        bestInInd = j;
                                    }
                                }

                            }
                        }
                    }
                    v1.route.add(i, cand);
                    v1.loading += cand.loading;
                }
            }
        }
        if (bestDeltaCost < -tolerance) {
            operIsUpdate = true;
            firCost += bestDeltaCost;
            bestOutV.route.remove(bestOutInd);
            bestInV.route.add(bestInInd, bestCand);
            bestOutV.loading -= bestCand.loading;
            bestInV.loading += bestCand.loading;
        }
    }

    //  relocate算子取出cust后车辆的固定成本减少量，由于车型是一样的，所以只考虑在路线中的情况，不考虑overDemand的用车
    private static double getMinusVFixCost(Vehicle v) {
        if (v.route.size() == 2) return v.fcost;
        else return 0;
    }

    //  relocate2算子取出cust后选址成本的下降量
    private static double getMinusCFixCost(SecondVehicle v) {
        if (v.route.size() == 2 && v.start_end.loading == v.loading) return v.start_end.cost;
        else return 0;
    }

    //  relocate算子取出cust后计算行驶成本的减少量(要考虑超量部分的运输成本的减少量)
    private static double getMinusTravelCost(Vehicle v, Customer now, Point last, Point next) {
        double cost = (dist[last.num][now.num]+dist[now.num][next.num]-dist[last.num][next.num])*v.vcost;
        Cand cand = (Cand)v.route.get(0);
        cost += 2*dist[cand.num][now.num]*v.vcost* (now.overDemand/v.cap);
        return cost;
    }

    private static double getMinusTravelCost(Vehicle v, Cand now, Point last, Point next) {
        double cost = (dist[last.num][now.num]+dist[now.num][next.num]-dist[last.num][next.num])*v.vcost;
        Depot depot = (Depot) v.route.get(0);
        cost += 2*dist[depot.num][now.num]*v.vcost* (now.overLoading/v.cap);
        return cost;
    }

    //  relocate算子插入cust后车辆数量的增加量
    private static double getAddVFixCost(Vehicle v) {
        if (v.route.size() == 2) return v.fcost;
        else return 0;
    }

    //  relocate算子插入cust后选址成本的增加量
    private static double getAddCFixCost(SecondVehicle v) {
        if (v.route.size() == 2 && v.start_end.loading == v.loading) return v.start_end.cost;
        else return 0;
    }

    //  relocate算子插入cust后计算行驶成本的增加量
    private static double getAddTravelCost(Vehicle v, Customer now, Point last, Point next) {
        return getMinusTravelCost(v,now,last,next);
    }

    private static double getAddTravelCost(Vehicle v, Cand now, Point last, Point next) {
        return getMinusTravelCost(v,now,last,next);
    }

    //  计算第二级路径的成本 = 车辆成本 + 行驶成本 + 选址成本
    public static double getSecCost() {
        double cost = 0;
        for (SecondVehicle v: secondVehicleList
        ) {
            if (v.route.size() == 2) continue;
            cost += v.fcost;
            for (int i = 0; i < v.route.size()-1; i++) {
                Point p1 = v.route.get(i);
                Point p2 = v.route.get(i+1);
                cost += v.vcost*dist[p1.num][p2.num];
                if (p1 instanceof Customer) {
                    Customer cust = (Customer) p1;
                    Cand cand = v.start_end;
                    cost += (double) cust.overDemand/v.cap * v.fcost;
                    cost += (double) cust.overDemand/v.cap * 2 * dist[cand.num][cust.num] * v.vcost;
                }
            }
        }
        //  计算网点开放成本
        for (Cand cand: openedCandSet
        ) {
            cost += cand.cost;
        }
        return cost;
    }

    //  计算一级路径的成本 = 车辆成本 + 行驶成本
    public static double getFirCost() {
        double cost = 0;
        double fix = 0;
        //  第一级路径的成本（车辆的固定成本+行驶成本）
        for (Vehicle v: firstVehicleList
        ) {
            if (v.route.size() == 2) continue;
            cost += v.fcost;
            fix += v.fcost;
            for (int i = 0; i < v.route.size()-1; i++) {
                Point p1 = v.route.get(i);
                Point p2 = v.route.get(i+1);
                cost += dist[p1.num][p2.num]*v.vcost;
                if (p1 instanceof Cand) {
                    Cand c = (Cand) p1;
                    Depot d = (Depot) v.route.get(0);
                    cost += (double) c.overLoading/v.cap * v.fcost;  //  对于每一个超量的cand，需要单独运输，计算车辆固定成本
                    cost += (double) c.overLoading/v.cap * 2 * dist[d.num][c.num] * v.vcost;  //  对于每一个超量的cand，需要单独运输，计算车辆行驶成本
                    fix += (double) c.overLoading/v.cap * v.fcost;
                }
            }
        }
        return cost;
    }

    /**
     * 第二级路径的邻域搜索
     */
    public static void NS2() {
        //int count = 0;
        secCost = getSecCost();
        int k = 0;
        while (k <= 10) {
            switch (k) {
                case 0:
                    setEmptyVehicle2();
                    relocate2();
                    //twoOpt2();
                    Check.check2();
                    //System.out.println(count);
                    //System.out.println(secCost);
                    //count ++;
                    if (operIsUpdate) operIsUpdate = false;
                    else k++;
                    break;
                case 1:
                    setEmptyVehicle2();
                    twoOpt2();
                    if (operIsUpdate) {
                        operIsUpdate = false;
                        k = 0;
                    }
                    else k++;
                    break;
                default:
                    k = Integer.MAX_VALUE;
            }
        }
        secondVehicleList.removeIf(v -> v.route.size() == 2);
    }

    /**
     * 第一级路径的邻域搜索
     */
    public static void NS1() {
        firstVehicleList.removeIf(v -> v.route.size() == 2);
        firCost = getFirCost();
        int k = 0;
        while (k <= 10) {
            switch (k) {
                case 0:
                    setEmptyVehicle1();
                    relocate1();
                    //Check.check1();
                    if (operIsUpdate) operIsUpdate = false;
                    else k++;
                    break;
                default:
                    k = Integer.MAX_VALUE;
            }
        }
        firstVehicleList.removeIf(v -> v.route.size() == 2);
    }

    /**
     * 第二级路径的变化导致cand的装载量发生变化，使得第一级路径中cand可能被移除或者导致路径超车装载量
     * 由于每一个cust都必须留下非0demand，因此一个cand一旦loading=0，其必然是空cand
     */
    public static void modify() {
        //  把空cand从open集合中删除
        //openedCandSet.removeIf(o -> o.loading == 0);
        //  将一级路径的数据更新，包括cand的loading，overLoading，一级车的loading(只需要更新一级车的loading)
        for (Cand cand: openedCandSet
        ) {
            cand.flashLoading(cap1);
        }
        for (FirstVehicle v: firstVehicleList
             ) {
            v.loading = 0;
            Iterator<Point> it = v.route.iterator();
            while (it.hasNext()) {
                Point p = it.next();
                if (p instanceof Cand) {
                    if (((Cand) p).loading == 0) it.remove();
                    else v.loading += ((Cand) p).loading;
                }
            }
        }
        //  对于路径中loading=0的cand，移除。对于路径中车的loading>cap，将路径分成两条
        int n = firstVehicleList.size();
        for (int i = 0; i < n; i++) {
            FirstVehicle v = firstVehicleList.get(i);
            if (v.loading > v.cap) {
                Cand removed = null;
                try{
                    removed = (Cand) v.route.remove(2);
                }catch (ClassCastException e) {
                    System.out.println(v.loading);
                    System.out.println(v.route);
                    System.out.println(((Cand)v.route.get(1)).loading);
                }

                v.loading -= removed.loading;
                FirstVehicle newV = new FirstVehicle(cap1,vFix1,vvar1);
                newV.add(v.start_end);
                newV.add(removed);
                newV.add(v.start_end);
                newV.start_end = v.start_end;
                firstVehicleList.add(newV);
            }
        }
    }

    private static void setEmptyVehicle2() {
        secondVehicleList.removeIf(v -> v.route.size() == 2);
        for (Cand c: openedCandSet
             ) {
            if (c.loading != 0) {
                SecondVehicle v = new SecondVehicle(cap2,vFix2,vvar2);
                v.start_end = c;
                v.route.add(c);
                v.route.add(c);
                secondVehicleList.add(v);
            }
        }
    }

    private static void setEmptyVehicle1() {
        firstVehicleList.removeIf(v -> v.route.size() == 2);
        for (Depot depot: depotSet
             ) {
            FirstVehicle v = new FirstVehicle(cap1,vFix1,vvar1);
            v.start_end = depot;
            v.route.add(depot);
            v.route.add(depot);
            firstVehicleList.add(v);
        }
    }

    /**
     * 最优解更新与否的评估
     */
    public static void optEvaluate() {
        if (firCost + secCost - lsObj.cost < -tolerance) {
            perturbNImp = 0;
            System.out.println("改进前的目标值："+lsObj.cost);
            getBetter = true;
            lsObj.cost = firCost+secCost;
            System.out.println("改进后的目标值："+lsObj.cost);
            lsObj.firCost = firCost;
            lsObj.secCost = secCost;
            lsObj.firVehicleList.clear();
            lsObj.secVehicleList.clear();
            lsObj.bestOpened.clear();
            for (FirstVehicle v: firstVehicleList
            ) {
                FirstVehicle vc = new FirstVehicle(cap1,vFix1,vvar1);
                vc.loading = v.loading;
                vc.start_end = v.start_end;
                vc.route.addAll(v.route);
                lsObj.firVehicleList.add(vc);
            }
            for (SecondVehicle v: secondVehicleList){
                SecondVehicle vc = new SecondVehicle(cap2,vFix2,vvar2);
                vc.loading = v.loading;
                vc.start_end = v.start_end;
                vc.route.addAll(v.route);
                lsObj.secVehicleList.add(vc);
            }
            lsObj.bestOpened.addAll(openedCandSet);
        }else perturbNImp += 1;
    }

    public static void NS() {
        restoreCandLoading();
        NS2();
        modify();
        NS1();
        updateOpenedCand();
        //Check.checkOpenedCand();
        //Check.checkCost1();
        optEvaluate();
    }

    public static void updateOpenedCand() {
        openedCandSet.removeIf(o -> o.loading == 0 && o.overLoading == 0);
    }
}
